//OpenCV
#include "cv.h"
#include "highgui.h"
//Other
#include <stdio.h>
#include "string.h"
#include "RGBDCamera.h"
#include <sstream>
#include <pthread.h>

const int max_frams = 1000000;
pthread_mutex_t mutex1 = PTHREAD_MUTEX_INITIALIZER;

using namespace cvap_6D_slam;
using namespace std;

void RGBDCamera::print(){
}

void RGBDCamera::addFrame(const cvap_6D_slam::kinect_data::ConstPtr& msg){
	
	pthread_mutex_lock( &mutex1 );

	rgb_list[nr_frames] = new string((msg->rgb).c_str());
	depth_list[nr_frames] = new string((msg->depth).c_str());
	timings[nr_frames] = msg->timing;
	nr_frames++;
	
	pthread_mutex_unlock( &mutex1 );
}

RGBDCamera::RGBDCamera(){
	nr_frames			= 0;
	rgb_list 			= new string * [max_frams];
	depth_list 			= new string * [max_frams];
	timings				= new double   [max_frams];
	ROS_INFO("max_frams: %i",max_frams);
	focal_length_x 		= 525.0f;
	focal_length_y 		= 525.0f;
	optical_center_x	= (640.0f-1.0f)/2.0f;
	optical_center_y	= (480.0f-1.0f)/2.0f;
}

RGBDCamera::~RGBDCamera(){}


RGBDFrame * RGBDCamera::getFrame(int i){
	pthread_mutex_lock( &mutex1 );
	if(i < 0 || i >= nr_frames){
		pthread_mutex_unlock( &mutex1 );
		return NULL;
	}
	pthread_mutex_unlock( &mutex1 );
	return new RGBDFrame(this,i);
}

RGBDFrame * RGBDCamera::getNextFrame(){return getFrame(current_frame_number++);}

RGBDFrame * RGBDCamera::getCurrentFrame(){return getFrame(current_frame_number);}

RGBDFrame * RGBDCamera::getPreviousFrame(){return getFrame(current_frame_number--);}

RGBDFrame * RGBDCamera::getLastFrame(){return getFrame(nr_frames-1);}

RGBDFrame * RGBDCamera::getFirstFrame(){return getFrame(0);}

